# -*- conding:utf-8 -*-

"""
@author: PoXiao
@file: traffic_fitness.py
@time: 2019/3/20 0020 16:52

"""
import numpy as np


def traffic_fitness(chromosome, Road, Car, Cross, crossMap):
    row = Car.shape[0]
    col = int(chromosome.shape[0] / row)
    chromosome = chromosome.reshape(row, col)
    allChromosome = chromosome.flatten()
    time = 0
    PositionData = np.zeros((Road.shape[0], max(Road[:,3]) * 2, max(Road[:, 1])), dtype=np.int)
    Car = np.c_[Car, np.zeros((Car.shape[0], 6))]
    Car = np.array(Car, dtype=np.int) #将Car里元素由float变为int
    while (1-((len(np.nonzero(Car[:,5])[0])) == len(Car[:,5]))) and time < 10000:
        time = time + 1
        for i in range(0, Car.shape[0]):
            if (1-Car[i][6]) and (Car[i][7] and (1-Car[i][5])):
                [Car, PositionData] = drive(crossMap, Car[i][0], chromosome, Car, Road, Cross, PositionData, time)
        for i in range(0, Car.shape[0]):
            if (1-Car[i][7]):
                cross_sequence = [0, find(np.where(chromosome[i,2:] == 1))+1, find(np.where(chromosome[i,2:] == 2))+1]
                road_next = crossMap[cross_sequence[1]-1, cross_sequence[2]-1]
                if isinstance(road_next, list) == True:  # 防止road_next取到-1
                    road_next.remove(-1)
                    road_next = road_next[0]
                else:
                    road_next = road_next
                S2 = min(Road[find(np.where(Road[:,0]==road_next))][2], Car[i][3])
                [Car, PositionData] = into_road(crossMap, Road, Car, Cross, PositionData, Car[i][0],
                                                cross_sequence, S2, time, chromosome)

        Car[:,6] = 0


    fitness = time
    return fitness

def behind_dog(Car, car_number, car_position, PositionData, dogcar_global):
    if dogcar_global[2] - car_position[2] != 1:
        PositionData[dogcar_global[0]][dogcar_global[1]][dogcar_global[2] - 1] = car_number
        temp = car_position[2]
        Car[find(np.where(Car[:,0] == car_number))][8:11] = [dogcar_global[0], dogcar_global[1], dogcar_global[2] - 1]
        PositionData[car_position[0]][car_position[1]][temp] = 0

    Car[find(np.where(Car[:,0] == car_number))][6] = 1
    return Car, PositionData

def creat_crossMap(Cross):
    cross_size = Cross.shape[0]  #获得Cross矩阵的行数
    crossMap = np.zeros((cross_size, cross_size))
    for i in range(0, cross_size):
        for j in range(0, cross_size):
            crossMap[i][j] = intersection(Cross[i][:], Cross[j][:])
            crossMap[i][i] = 0
    return crossMap

def creat_roadMap(Road,crossMap):
    addColumn = np.zeros((Road.shape[0],4))
    d = np.c_[Road, addColumn]
    for i, j in zip(range(Road[0][0], Road[Road.shape[0] - 1][0] + 1), range(0, Road.shape[0])):
        d[j,7:9] = np.where(crossMap == i)[1]
        d[j,9:11] = np.where(crossMap == i)[0]

    Road = d
    return Road

def cross2road(crossMap, Road, Car, Cross, chromosome, car_number, CarPosition):
    crossOFchromosome = chromosome[chromosome[:,0] == car_number, 2:]
    road_now = Road[Car[find(np.where(Car[:,0]==car_number))][8]][0]
    cross_now = Road[find(np.where(Road[:,0]==road_now))][7:9]

    if (crossOFchromosome[0, cross_now[0]-1]) > (crossOFchromosome[0, cross_now[1]-1]):
        cross_now = [cross_now[1], cross_now[0]]

    idxNextCross = crossOFchromosome[0, cross_now[1]-1] + 1
    cross_sequence = []
    cross_sequence.extend(cross_now)  #将列表添加到空列表中
    cross_sequence.append(find(np.where(crossOFchromosome[0,:] == idxNextCross))+1) #单个数值添加到列表中
    road_next = -1 #初始值
    # if len(cross_sequence) == 3:
    road_next = crossMap[cross_sequence[1]-1][cross_sequence[2]-1]
    # else:
    #     road_next = -1

    idx_now = find(np.where(Cross[cross_sequence[1]-1,:]==road_now))
    idx_next = find(np.where(Cross[cross_sequence[1]-1,:]==road_next))

    direct = -1 #作为初始值
    if (type(road_next)) == np.ndarray:
        direct = -1
    elif abs(idx_now - idx_next) == 2:
        direct = 1
    elif (idx_now - idx_next == -1) or (idx_now - idx_next == 3):
        direct = 2
    elif (idx_now - idx_next == 1) or (idx_now - idx_next == -3):
        direct = 3
    else:
        print("Direct Error")

    return road_now, road_next, direct, cross_sequence

def drive(crossMap,car_number, chromosome, Car, Road, Cross, PositionData, time):
    if Car[find(np.where(Car[:,0] == car_number))][6] == 1:
        print("Drive Error!!!")
    car_position = Car[find(np.where(Car[:, 0] == car_number))][8:11]
    road_rest = PositionData[car_position[0]][car_position[1]][(car_position[2]+1) : (Road[car_position[0], 1])]
    # road_rest = road_rest.flatten()
    # road_rest = road_rest.reshape(road_rest.size)
    # road_rest = road_rest[::-1]
    # road_rest = road_rest.reshape(road_rest.shape)
    dogcar_local = np.nonzero(road_rest)[0]
    dogcar_number = []
    if len(dogcar_local) > 0:
        dogcar_local = dogcar_local[0] #如果dogcar_local不为空，返回第一个不为0元素的下标
        dogcar_global = car_position[0:2]
        dogcar_global = np.r_[dogcar_global, car_position[2] + dogcar_local + 1]
        dogcar_number = PositionData[dogcar_global[0]][dogcar_global[1]][dogcar_global[2]]
        # dog_chromosome = getcar_cho(Road, Car, Cross, chromosome, dogcar_number)

    [road_now, road_next, direct, cross_sequence] = cross2road(crossMap, Road, Car, Cross, chromosome, car_number, car_position)
    R1 = Road[find(np.where(Road[:,0] == road_now)), 2]
    R2 = Road[find(np.where(Road[:,0] == road_next)), 2]
    V = Car[find(np.where(Car[:,0] == car_number)), 3]
    V1 = min(R1, V)
    V2 = min(R2, V)
    SV1 = V1
    SV2 = V2
    S1 = Road[find(np.where(Road[:,0] == road_now))][1] - (car_position[2]+1)
    S2 = max(0, SV2 - S1)

    if S1 < V1:
        if (type(dogcar_local)) != np.ndarray:
            if Car[find(np.where(Car[:,0] == dogcar_number))][6] == 1:
                [Car, PositionData] = behind_dog(Car, car_number, car_position, PositionData, dogcar_global)
            else:
                [Car, PositionData] = drive(crossMap, dogcar_number, chromosome, Car, Road, Cross, PositionData, time)
                [Car, PositionData] = drive(crossMap, car_number, chromosome, Car, Road, Cross, PositionData, time)
        else:
            if (type(road_next)) == np.ndarray:
                PositionData[car_position[0]][car_position[1]][car_position[2]] = 0
                Car[find(np.where(Car[:,0]==car_number))][8:11] = [-1, -1, -1]
                Car[find(np.where(Car[:,0]==car_number))][5] = 1
            elif S2:
                [Car, PositionData] = fuck_cross_test(crossMap, Road, Car, Cross, PositionData, car_number, chromosome, car_position, time)
            else:
                temp = car_position[2] + V1
                temp2 = car_position[2]
                PositionData[car_position[0]][car_position[1]][temp] = car_number
                Car[find(np.where(Car[:,0]==car_number))][8:11] = [car_position[0], car_position[1], temp]
                PositionData[car_position[0]][car_position[1]][temp2] = 0
    else:
        if (type(dogcar_local)) != np.ndarray:
            if Car[find(np.where(Car[:,0] == dogcar_number))][6] == 1:
                [Car, PositionData] = behind_dog(Car, car_number, car_position, PositionData, dogcar_global)
            else:
                [Car, PositionData] = drive(crossMap, dogcar_number, chromosome, Car, Road, Cross, PositionData, time)
                [Car, PositionData] = drive(crossMap, car_number, chromosome, Car, Road, Cross, PositionData, time)
        else:
            temp = car_position[2] + V1
            temp2 = car_position[2]
            PositionData[car_position[0]][car_position[1]][temp] = car_number
            Car[find(np.where(Car[:,0]==car_number))][8:11] = [car_position[0], car_position[1], temp]
            PositionData[car_position[0]][car_position[1]][temp2] = 0
            Car[find(np.where(Car[:,0] == car_number))][6] = 1

    return Car, PositionData

def examine_car_enableRoad(Road,Car, car_number, PositionData, road_now, road_next):
    car_position = Car[find(np.where(Car[:,0] == car_number))][8:11]          # 8:11 末尾11取不到  8  9  10
    car_d = car_position[2]
    road_now_row = find(np.where(Road[:,0] == road_now))
    R1 = Road[road_now_row][2]
    nowRoadLength = Road[road_now_row][1]
    road_next_row = find(np.where(Road[:,0] == road_next))
    R2 = Road[road_next_row][2]
    V = Car[find(np.where(Car[:,0] == car_number))][3]
    V1 = min(R1, V)
    V2 = min(R2, V)
    S1 = nowRoadLength - (car_d+1)
    S2 = max(0, V2 - S1)
    if S2 > 0:
        flag = 1
    else:
        flag = -1

    return flag, S2

def find(tuple):
    npwhere = tuple[0]
    if len(npwhere) > 1:
        return npwhere
    elif len(npwhere) == 1:
        return npwhere[0]
    else:
        return npwhere
def find_road_firstcar(Road, Road_Number, PositionData, lane):
    car_number = -1
    roadLength = 0
    road_row = find(np.where(Road[:,0] == Road_Number))
    if road_row:
        roadNumber = Road[road_row][3]
        roadBothway = Road[road_row][6]
        roadLength = Road[road_row][1]
        Road_Number = find(np.where(Road[:,0] == Road_Number))

    if roadLength:
        if roadBothway == 0:
            for j in range(roadLength-1, -1, -1):
                flag = 0
                for i in range(0, roadNumber):
                    if PositionData[Road_Number][i][j] != 0:
                        car_number = PositionData[Road_Number][i][j]
                        flag = 1
                        break
                    else:
                        car_number = -1
                if flag == 1:
                    break
        else:
            if lane > 0:
                for j in range(roadLength-1, -1, -1):
                    flag = 0
                    for i in range(0, roadNumber):
                        if PositionData[Road_Number][i][j] != 0:
                            car_number = PositionData[Road_Number][i][j]
                            flag = 1
                            break
                        else:
                            car_number = -1
                    if flag == 1:
                        break
            else:
                for j in range(roadLength-1, -1, -1):
                    flag = 0
                    for i in range(roadNumber, roadNumber*2):
                        if PositionData[Road_Number][i][j] != 0:
                            car_number = PositionData[Road_Number][i][j]
                            flag = 1
                            break
                        else:
                            car_number = -1
                    if flag == 1:
                        break
    return car_number
def fuck_cross_test(crossMap,Road, Car, Cross, PositionData, car_number, chromosome, CarPosition, time):
    [road_now, road_next, direct, cross_sequence] = cross2road(crossMap, Road, Car, Cross, chromosome, car_number, CarPosition)
    road_now_row = find(np.where(Road[:,0] == road_now))
    R1 = Road[road_now_row][2]
    nowRoadLength = Road[road_now_row][1]
    road_next_row = find(np.where(Road[:,0] == road_next))
    R2 = Road[road_next_row][2]

    V = Car[find(np.where(Car[:,0] == car_number))][3]
    V1 = min(R1, V)
    V2 = min(R2, V)
    car_position = Car[find(np.where(Car[:,0]==car_number))][8:11]
    car_r = car_position[0]
    car_l = car_position[1]
    car_d = car_position[2]
    S1 = nowRoadLength - (car_d+1)
    S2 = max(0, V2 - S1)
    c2 = cross_sequence[1]

    if S2 == 0:
        PositionData[car_r][car_l][car_d + S1] = car_number
        PositionData[car_r][car_l][car_d] = 0
        Car[find(np.where(Car[:,0] == car_number))][6] = 1
    else:
        if direct == 1:
            [Car, PositionData] = into_road(crossMap,Road,Car,Cross, PositionData, car_number,cross_sequence, S2, time, chromosome)
        elif direct == 2:
            temp = find(np.where(Cross[find(np.where(Cross==c2)),:] == road_next))
            if temp > 2:
                temp = temp - 2
            else:
                temp = temp + 2
            c2_row = find(np.where(Cross == c2))
            blockingRoad = Cross[c2_row][temp]
            if blockingRoad == -1:
                [Car, PositionData] = into_road(crossMap, Road, Car, Cross, PositionData, car_number, cross_sequence, S2, time, chromosome)
            else:
                if (Road[find(np.where(Road[:,0]==blockingRoad)),6] == 1) or (Road[find(np.where(Road[:,0])==blockingRoad),6] == 0 and Road[find(np.where(Road[:,0])==blockingRoad),6] == c2):
                    if blockingRoad < road_next:
                        lane = 1
                    else:
                        lane = -1
                    blockingCar = find_road_firstcar(Road, blockingRoad, PositionData, lane)
                    if blockingCar == -1:
                        [Car, PositionData] = into_road(crossMap, Road, Car, Cross, PositionData, car_number, cross_sequence, S2, time, chromosome)
                    else:
                        blocking_car_position = Car[find(np.where(Car[:,0] == blockingCar))][8:11]
                        m = blocking_car_position[0]
                        n = blocking_car_position[1]
                        p = blocking_car_position[2]
                        # blockingChromosome = getcar_cho(Road, Car, Cross, blockingCar)
                        [blocking_road_now, blocking_road_next, blocking_direct, blocking_cross_sequence] = cross2road(crossMap, Road, Car, Cross, chromosome, blockingCar, blocking_car_position)
                        [enable, blockingCar_S2] = examine_car_enableRoad(Road, Car, blockingCar, PositionData, blocking_road_now, blocking_road_next)
                        if (blocking_direct == 1 and enable > 0 and Car[find(np.where(Car == PositionData[m][n][p])), 6] == 0):
                            [Car, PositionData] = into_road(crossMap, Road, Car, Cross, PositionData, blockingCar, blocking_cross_sequence, blockingCar_S2, time, chromosome)
                            [Car, PositionData] = fuck_cross_test(crossMap, Road, Car, Cross, PositionData, car_number, chromosome, CarPosition, time)
                        else:
                            [Car, PositionData] = into_road(crossMap, Road, Car, Cross, PositionData, car_number, cross_sequence, S2, time, chromosome)
                else:
                    if Road[find(np.where(Road[:,0] == blockingRoad))][5] != c2:
                        [Car, PositionData] = into_road(crossMap, Road, Car, Cross, PositionData, car_number, cross_sequence, S2, time, chromosome)
        elif direct == 3:
            temp1 = find(np.where(Cross[find(np.where(Cross == c2)),:] == road_next))
            if temp1 > 2:
                temp1 = temp1 - 2
            else:
                temp1 = temp1 + 2
            temp2 = find(np.where(Cross[find(np.where(Cross == c2)),:] == road_now))
            if temp2 > 2:
                temp2 = temp2 - 2
            else:
                temp2 = temp2 + 2
            blockingRoad1 = Cross[find(np.where(Cross == c2))][temp1]
            blockingRoad2 = Cross[find(np.where(Cross == c2))][temp2]
            if blockingRoad1 == -1:
                blockingRoadLane1 = []
            else:
                blockingRoadLane1 = Road[find(np.where(Road[:,0] == blockingRoad1))][6]
            if blockingRoad2 == -1:
                blockingRoadLane2 = []
            else:
                blockingRoadLane2 = Road[find(np.where(Road[:,0] == blockingRoad2))][6]
            if blockingRoad1 < road_next:
                lane1 = 1
            else:
                lane1 = -1
            if blockingRoad2 < road_next:
                lane2 = 1
            else:
                lane2 = -1
            blockingCar1 = find_road_firstcar(Road, blockingRoad1, PositionData, lane1)
            blockingCar2 = find_road_firstcar(Road, blockingRoad2, PositionData, lane2)
            direct1 = 0  #作为初始化
            direct2 = 0
            cross_sequence1 = 0 #作为初始化
            cross_sequence2 = 0
            CarChromosome1 = 0
            CarChromosome2 = 0
            if blockingCar1 != -1:
                CarPosition1 = Car[find(np.where(Car[:,0] == blockingCar1))][8:11]
                # CarChromosome1 = getcar_cho(Road, Car, Cross, blockingCar1)
                [road_now1, road_next1, direct1, cross_sequence1] = cross2road(crossMap, Road, Car, Cross, chromosome, blockingCar1, CarPosition1)
                [enable1, obstructionS2_1] = examine_car_enableRoad(Road, Car, blockingCar1, PositionData, road_now1, road_next1)
            else:
                enable1 = -1

            if blockingCar2 != -1:
                CarPosition2 = Car[find(np.where(Car[:,0] == blockingCar2))][8:11]
                # CarChromosome2 = getcar_cho(Road, Car, Cross, chromosome, blockingCar2)
                [road_now2, road_next2, direct2, cross_sequence2] = cross2road(crossMap, Road, Car, Cross, chromosome, blockingCar2, CarPosition2)
                [enable2, obstructionS2_2] = examine_car_enableRoad(Road, Car, blockingCar2, PositionData, road_now2, road_next2)
            else:
                enable2 = -1

            if (blockingRoad1 != -1) and (blockingCar1 != -1) and (direct1 == 1) and (enable1 == 1) and (Car[find(np.where(Car[:,0] == blockingCar1)),6] == 0) and (blockingRoadLane1 == 1 or (blockingRoadLane1 == 0 and Road[find(np.where(Road[:,0] == blockingRoad1)),5] == c2)):
                [Car, PositionData] = into_road(crossMap, Road, Car, Cross, PositionData, blockingCar1,
                                                cross_sequence1, obstructionS2_1, time, chromosome)
                [Car, PositionData] = fuck_cross_test(crossMap, Road, Car, Cross, PositionData,
                                                      car_number, chromosome, CarPosition, time)
            elif (blockingRoad2 != -1) and (blockingCar2 != -1) and (direct2 == 2) and (enable2 == 1) and (Car[find(np.where(Car[:,0] == blockingCar2)),6] == 0) and (blockingRoadLane2 == 1 or (blockingRoadLane2 == 0 and Road[find(np.where(Road[:,0] == blockingRoad2)),5] == c2)):
                [Car, PositionData] = into_road(crossMap, Road, Car, Cross, PositionData, blockingCar2,
                                                cross_sequence2, obstructionS2_2, time, chromosome)
                [Car, PositionData] = fuck_cross_test(crossMap, Road, Car, Cross, PositionData,
                                                    car_number, chromosome, CarPosition, time)
            else:
                [Car, PositionData] = into_road(crossMap, Road, Car, Cross, PositionData, car_number,
                                                cross_sequence, S2, time, chromosome)
    return Car, PositionData

# def getcar_cho(Road, Car, Cross, chromosome ,car_number):
#     # row = int(Car.shape[0])
#     # col = int(len(allChromosome) / 8)
#     # temp = allChromosome.reshape(row, col)
#     temp = chromosome[chromosome[:,0] == car_number,:]
#     carChromosome = temp[find(np.where(temp[:,0] == car_number)),:]
#     return carChromosome

def intersection(list1, list2):
    for i in range(1, 4):
        for j in range(1, 4):
            if (list2[j] == list1[i]) and (list2[j] != -1):
                number = list2[j]
                return number
            else:
                number = 0

    return number

def into_road(crossMap,Road,Car,Cross, PositionData, car_number,cross_sequence, S2, time, chromosome):
    c1 = cross_sequence[0]
    c2 = cross_sequence[1]
    c3 = cross_sequence[2]
    # temp1 = Cross[c2, :]
    # temp2 = Cross[c3, :]
    #goalRoadNumber = intersection(temp1, temp2)  #取交集
    goalRoadNumber = crossMap[c2-1][c3-1]
    if isinstance(goalRoadNumber, list) == True:    #防止road_next取到-1
        goalRoadNumber.remove(-1)
        road_next = goalRoadNumber[0]
    else:
        road_next = goalRoadNumber
    road_next_row = find(np.where(Road[:,0] == road_next))
    goalRoadNumber = Road[road_next_row][3]  #目标车道数目
    emptySpaceRoadNumber = -1
    S2 = int(S2)
    if c3 - c2 > 0:
        for i in range(0, goalRoadNumber):    #i 从0到goalRoadNumber-1
            flag = 0
            for j in range(0, S2):
                if (PositionData[road_next_row][i][j]) == 0:
                    if j == (S2-1):
                        emptySpaceRoadNumber = i
                        emptySpaceRoadLength = j
                        flag = 1
                        break
                else:
                    if Car[find(np.where(Car[:, 0] == PositionData[road_next_row][i][j]))][6] == 0:   #表示在道上的车没被开过
                        [Car, PositionData] = drive(crossMap, PositionData[road_next_row, i, j], chromosome, Car, Road, Cross, PositionData, time)
                        [Car, PositionData] = into_road(crossMap, Road, Car, Cross, PositionData, car_number, cross_sequence, S2, time, chromosome)
                        return Car, PositionData
                    elif Car[find(np.where(Car[:,0] == PositionData[road_next_row][i][j]))][6] == 1:
                        if j == 0:
                            break
                        else:
                            emptySpaceRoadNumber = i
                            emptySpaceRoadLength = j - 1
                            flag = 1
                            break
            if flag == 1:
                break
    else:
        for i in range(goalRoadNumber, goalRoadNumber*2):    #i 从0到goalRoadNumber-1
            flag = 0
            for j in range(0, S2):
                if (PositionData[road_next_row][i][j]) == 0:
                    if j == (S2-1):
                        emptySpaceRoadNumber = i
                        emptySpaceRoadLength = j
                        flag = 1
                        break
                else:
                    if Car[find(np.where(Car[:,0] == PositionData[road_next_row][i][j]))][6] == 0:   #表示在道上的车没被开过
                        [Car, PositionData] = drive(crossMap, PositionData[road_next_row][i][j], chromosome, Car, Road, Cross, PositionData, time)
                        [Car, PositionData] = into_road(crossMap, Road, Car, Cross, PositionData, car_number, cross_sequence, S2, time, chromosome)
                        return Car, PositionData
                    elif Car[find(np.where(Car[:,0] == PositionData[road_next_row][i][j]))][6] == 1:
                        if j == 0:
                            break
                        else:
                            emptySpaceRoadNumber = i
                            emptySpaceRoadLength = j - 1
                            flag = 1
                            break
            if flag == 1:
                break
    car_position = Car[find(np.where(Car[:,0] == car_number))][8:11]
    car_r = car_position[0]
    car_l = car_position[1]
    car_d = car_position[2]
    Car_row = find(np.where(Car[:,0] == car_number))
    if emptySpaceRoadNumber == -1:
        if Car[Car_row][7] != 0:
            roadEndPosition = Road[car_r][1]
            PositionData[car_r][car_l][car_d] = 0
            PositionData[car_r][car_l][roadEndPosition-1] = car_number
            Car[Car_row][8:11] = [car_r][car_l][roadEndPosition]
            Car[Car_row][6] = 1
    else:
        if Car[Car_row][7] != 0:
            PositionData[car_r][car_l][car_d] = 0
        PositionData[road_next_row][emptySpaceRoadNumber][emptySpaceRoadLength] = car_number
        Car[Car_row][8:11] = [road_next_row, emptySpaceRoadNumber, emptySpaceRoadLength]
        Car[Car_row][6] = 1
        if Car[Car_row][7] == 0:
            Car[Car_row][7] = time


    return Car, PositionData